#include "trajectory/trajectory.h"

using namespace trajectory;

Trajectory::Trajectory(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
: nh_(nh),
  private_nh_(private_nh_)
{
    sub_task_ = nh_.subscribe("/task", 10, &Trajectory::TaskCallback, this);
    pub_trajectory_ = nh_.advertise<amr_trajectory_msgs::amr_trajectory_msg>("/trajectory", 10);
    Init();
}

Trajectory::~Trajectory()
{

}

void Trajectory::Init()
{
    // common_lib::SpdLog::Instance()->InitSpdLog(file_path_+"/log", "map");

    // 加载参数文件
    // YAML::Node vehicle_params = YAML::LoadFile(param_file_);
    step_size_ = 0.02f; //vehicle_params["step_size"].as<double>();
    amr_acc_ = 0.5f;
    amr_dcc_ = -0.5f;

   
}

void Trajectory::Start()
{
    ros::Rate loop(10);
    ros::Time start = ros::Time::now();
    bool send = true;
    while (ros::ok())
    {
        TrajectoryProcess();

        ros::spinOnce();
        loop.sleep();
    }
}

void Trajectory::GenerateTaskMsg(void)
{
    trajectory::TaskClass task_class;
    task_msgs::task_msg msg;
    msg.target_pointid = "3";
    task_msgs::task_path path;
    path.pathid = "1";
    path.max_speed = 0.5f;
    path.move_direction = "forward";
    path.pathtype = "line";
    path.task_id = 1;
    path.amr_angle = 0.0f;
    task_msgs::task_point point;
    point.pointid = "1";
    point.pose.position.x = 6.56f;
    point.pose.position.y = 4.14f;
    point.pose.position.z = 0.0f;
    path.control_points.push_back(point);
    point.pointid = "2";
    point.pose.position.x = 9.76f;
    point.pose.position.y = 4.14f;
    point.pose.position.z = 0.0f;
    path.control_points.push_back(point);
    path.key_points = path.control_points;
    msg.task.push_back(path);

    path.control_points.clear();
    path.key_points.clear();
    // msg.task.clear();
    path.pathid = "2";
    path.max_speed = 0.5f;
    path.move_direction = "forward";
    path.pathtype = "line";
    path.task_id = 1;
    path.amr_angle = 0.0f;
    point.pointid = "2";
    point.pose.position.x = 9.76f;
    point.pose.position.y = 4.14f;
    point.pose.position.z = 0.0f;
    path.control_points.push_back(point);
    point.pointid = "3";
    point.pose.position.x = 12.96f;
    point.pose.position.y = 4.14f;
    point.pose.position.z = 0.0f;
    path.control_points.push_back(point);
    path.key_points = path.control_points;
    msg.task.push_back(path);
    
    task_class.task = msg;
    task_class.target_point_id = "3";
    task_queue_.push(task_class);
}

void Trajectory::TaskCallback(const task_msgs::task_msgConstPtr &msg)
{
    trajectory::TaskClass task_class;

    if (!msg->task.empty())
    {
        task_class.task = *msg;
        task_class.target_point_id = msg->target_pointid;
        task_class.task_id = msg->task_id;
        ROS_INFO("TaskCallback() --> %s, size: %d", msg->target_pointid.c_str(), (int)msg->task.size());
        // for (auto task: msg->task)
        // {
        //     ROS_WARN("pathid: %s", task.pathid.c_str());
        // }
        task_queue_.push(task_class);
    }
}

void Trajectory::TrajectoryProcess()
{
    if (!task_queue_.empty())
    {
        TaskClass task_tmp = task_queue_.front();
        task_queue_.pop();
        task_trajectory_.target_pointid = task_tmp.target_point_id;
        task_trajectory_.task_id = task_tmp.task_id;
        for (auto path : task_tmp.task.task)
        {
            if (um_path_.find(path.pathtype) != um_path_.end())
            {
                ROS_INFO("TrajectoryProcess() pathid: %s, dir: %s, amr_angle: %f, maxv: %f", path.pathid.c_str(), path.move_direction.c_str(), path.amr_angle, path.max_speed);
                amr_trajectory_msgs::amr_trajectory_msg trajectory = (this->*um_path_[path.pathtype])(path, path.pathid);

                if (!task_trajectory_.points.empty())
                {
                    if (RepeatDetection(trajectory.points[0].pose, task_trajectory_))
                    {
                        trajectory.points.erase(trajectory.points.begin());
                    }
                }
                task_trajectory_.points.insert(task_trajectory_.points.end(), trajectory.points.begin(), trajectory.points.end());
            }
        }

        CalcHeading(task_trajectory_);
        SpeedPlan(task_trajectory_, amr_acc_, amr_acc_);
        // for (auto point : task_trajectory_.points)
        // {
        //     printf("point: x: %f, y: %f, theta: %f, angle: %f, dis: %f, direction: %s, v: %f, type: %s\n", 
        //             point.pose.position.x, point.pose.position.y, tf::getYaw(point.pose.orientation), point.amr_angle,
        //             point.distance, point.move_direction.c_str(), point.target_speed, point.angle_type.c_str());
        // }
        task_trajectory_.header.stamp = ros::Time::now();
        pub_trajectory_.publish(task_trajectory_);

        task_trajectory_.points.clear();
        task_trajectory_.target_pointid = "";
        task_trajectory_.task_id = 0;
    }
}

void Trajectory::MoveTaskProcess()
{

}

void Trajectory::WorkTaskProcess()
{

}

// 直线轨迹离散
amr_trajectory_msgs::amr_trajectory_msg Trajectory::LinePro(task_msgs::task_path &path, std::string id)
{
    amr_trajectory_msgs::amr_trajectory_msg trajectory;
    if (path.control_points.size() == 2)
    {
        double dis = std::hypotf(path.control_points[0].pose.position.x-path.control_points[1].pose.position.x, path.control_points[0].pose.position.y-path.control_points[1].pose.position.y);

        int cnt = 0;
        amr_trajectory_msgs::point_info tmp_point;
        double s = fabs(path.control_points[1].pose.position.y - path.control_points[0].pose.position.y) / dis;
        double c = fabs(path.control_points[1].pose.position.x - path.control_points[0].pose.position.x) / dis;

        while (cnt * step_size_ < dis)
        {
            tmp_point.pathid = path.pathid;
            if (path.control_points[1].pose.position.x > path.control_points[0].pose.position.x)
                tmp_point.pose.position.x = path.control_points[0].pose.position.x + cnt * step_size_ * c;
            else
                tmp_point.pose.position.x = path.control_points[0].pose.position.x - cnt * step_size_ * c;

            if (path.control_points[1].pose.position.y > path.control_points[0].pose.position.y)
                tmp_point.pose.position.y = path.control_points[0].pose.position.y + cnt * step_size_ * s;
            else
                tmp_point.pose.position.y = path.control_points[0].pose.position.y - cnt * step_size_ * s;
            
            tmp_point.curvature = 0.0f;
            float dis_to_start = std::hypotf(tmp_point.pose.position.x-path.key_points[0].pose.position.x, tmp_point.pose.position.y-path.key_points[0].pose.position.y);
            float dis_to_end   = std::hypotf(tmp_point.pose.position.x-path.key_points[1].pose.position.x, tmp_point.pose.position.y-path.key_points[1].pose.position.y);
            if (dis_to_start > point_id_dis_precision_ && dis_to_end > point_id_dis_precision_)
            {
                tmp_point.waypoint_id = "";
            }
            else if (dis_to_start > dis_to_end)
            {
                tmp_point.waypoint_id = path.key_points[1].pointid;
            }
            else
            {
                tmp_point.waypoint_id = path.key_points[0].pointid;
            }
            tmp_point.amr_angle      = path.amr_angle;
            tmp_point.angle_type     = path.angle_type;
            tmp_point.move_direction = path.move_direction;
            tmp_point.target_speed   = path.max_speed;

            if (!RepeatDetection(tmp_point.pose, trajectory))
                trajectory.points.push_back(tmp_point);            
            cnt++;
        }
        tmp_point.pathid = path.pathid;
        tmp_point.pose.position.x = path.control_points[1].pose.position.x;
        tmp_point.pose.position.y = path.control_points[1].pose.position.y;
        float dis_to_start = std::hypotf(tmp_point.pose.position.x-path.key_points[0].pose.position.x, tmp_point.pose.position.y-path.key_points[0].pose.position.y);
        float dis_to_end   = std::hypotf(tmp_point.pose.position.x-path.key_points[1].pose.position.x, tmp_point.pose.position.y-path.key_points[1].pose.position.y);
        if (dis_to_start > point_id_dis_precision_ && dis_to_end > point_id_dis_precision_)
        {
            tmp_point.waypoint_id = "";
        }
        else if (dis_to_start > dis_to_end)
        {
            tmp_point.waypoint_id = path.key_points[1].pointid;
        }
        else
        {
            tmp_point.waypoint_id = path.key_points[0].pointid;
        }
        tmp_point.amr_angle = path.amr_angle;
        tmp_point.angle_type = path.angle_type;
        tmp_point.move_direction = path.move_direction;
        tmp_point.target_speed   = path.max_speed;

        if (!RepeatDetection(tmp_point.pose, trajectory))
            trajectory.points.push_back(tmp_point);
    }
    
    // printf("LinePro() --> size: %d\n", (int)trajectory.points.size());
    return trajectory;
}

amr_trajectory_msgs::amr_trajectory_msg Trajectory::CirclePro(task_msgs::task_path &path, std::string id)
{
    amr_trajectory_msgs::amr_trajectory_msg trajectory;
    return trajectory;
}

amr_trajectory_msgs::amr_trajectory_msg Trajectory::BsplinePro(task_msgs::task_path &path, std::string id)
{
    amr_trajectory_msgs::amr_trajectory_msg trajectory;
    return trajectory;    
}

bool Trajectory::RepeatDetection(geometry_msgs::Pose new_point, amr_trajectory_msgs::amr_trajectory_msg &trajectory)
{
    if (trajectory.points.size() == 0)
    {
        return false;
    }
    float dis = std::hypot(new_point.position.x-trajectory.points[trajectory.points.size()-1].pose.position.x, new_point.position.y-trajectory.points[trajectory.points.size()-1].pose.position.y);
    if (dis < 0.001)
        return true;
    else
        return false;
}


void Trajectory::CalcHeading(amr_trajectory_msgs::amr_trajectory_msg &trajectory)
{
    if (trajectory.points.empty())
        return;

    uint32_t start = 0;

    trajectory.points[0].distance = 0.0f;
    float last_theta = atan2(trajectory.points[1].pose.position.y-trajectory.points[0].pose.position.y, trajectory.points[1].pose.position.x-trajectory.points[0].pose.position.x);
    trajectory.points[0].pose.orientation = tf::createQuaternionMsgFromYaw(last_theta);

    if (trajectory.points[0].angle_type == "turn")
    {
        trajectory.points[0].amr_angle = last_theta;
        if (trajectory.points[0].move_direction == "backward")
        {
            trajectory.points[0].amr_angle = com_.AngleConstraintToPI((float)(trajectory.points[0].amr_angle + M_PI));
        }
    }
    else if (trajectory.points[0].angle_type == "shift")
    {
        if (trajectory.points[0].move_direction == "backward")
        {
            last_theta = com_.AngleConstraintToPI(last_theta + M_PI);
            trajectory.points[0].pose.orientation = tf::createQuaternionMsgFromYaw(last_theta);
        }
    }

    uint32_t size = trajectory.points.size();

    for (int i=1;i<size-1;i++)
    {

        float theta = 0.0f;
        if (trajectory.points[i].pathid == trajectory.points[i+1].pathid)
        {
            theta = atan2(trajectory.points[i+1].pose.position.y-trajectory.points[i].pose.position.y, trajectory.points[i+1].pose.position.x-trajectory.points[i].pose.position.x);
        }
        else
        {
            theta = atan2(trajectory.points[i].pose.position.y-trajectory.points[i-1].pose.position.y, trajectory.points[i].pose.position.x-trajectory.points[i-1].pose.position.x);
        }
        trajectory.points[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);

        if (trajectory.points[i].angle_type == "turn")
        {
            trajectory.points[i].amr_angle = theta;
            if (trajectory.points[i].move_direction == "backward")
            {
                trajectory.points[i].amr_angle = com_.AngleConstraintToPI((float)(trajectory.points[i].amr_angle + M_PI));
            }
        }
        else if (trajectory.points[i].angle_type == "shift")
        {
            // if (trajectory.points[i].move_direction == "backward")
            // {
            //     theta = com_.AngleConstraintToPI(theta + M_PI);
            //     trajectory.points[i].pose.orientation = tf::createQuaternionMsgFromYaw(theta);
            // }
        }

        trajectory.points[i].distance = trajectory.points[i-1].distance + 
                                        std::hypot(trajectory.points[i].pose.position.x-trajectory.points[i-1].pose.position.x, 
                                                   trajectory.points[i].pose.position.y-trajectory.points[i-1].pose.position.y);

        if (fabs(last_theta-theta)>FLT_EPSILON)  // TODO Add Shift Mode
        {
            SpeedPlanRecord tmp;
            tmp.end = i-1;
            tmp.max_speed = trajectory.points[i-1].target_speed;
            tmp.move_direction = trajectory.points[i-1].move_direction;
            um_record_[start] = tmp;
            start = i-1;
        }
        last_theta = theta;
    }
    trajectory.points[size-1].distance = trajectory.points[size-2].distance + 
                                         std::hypot(trajectory.points[size-1].pose.position.x-trajectory.points[size-2].pose.position.x, 
                                                    trajectory.points[size-1].pose.position.y-trajectory.points[size-2].pose.position.y);
                                        
    trajectory.points[size-1].pose.orientation = trajectory.points[size-2].pose.orientation;
    trajectory.points[size-1].amr_angle        = trajectory.points[size-2].amr_angle;

    SpeedPlanRecord tmp;
    tmp.end = size-1;
    tmp.move_direction = trajectory.points[size-1].move_direction;
    tmp.max_speed = trajectory.points[size-1].target_speed;
    um_record_[start] = tmp;

    // for (auto m : um_record_)
    // {
    //     printf("start:%d end: %d, direction: %s, targetv: %f\n", m.first, m.second.end, m.second.move_direction.c_str(), m.second.max_speed);
    // }
}

void Trajectory::CalcDistance(amr_trajectory_msgs::amr_trajectory_msg &trajectory)
{
    if (trajectory.points.empty())
        return;
    
    trajectory.points[0].distance = 0.0f;
    std::vector<amr_trajectory_msgs::point_info>::iterator iter = trajectory.points.begin();
    for (std::vector<amr_trajectory_msgs::point_info>::iterator iter=trajectory.points.begin()+1;iter!=trajectory.points.end();iter++)
    {
        iter->distance = (iter-1)->distance + std::hypot(iter->pose.position.x-(iter-1)->pose.position.x, iter->pose.position.y-(iter-1)->pose.position.y);
    }
}

void Trajectory::SpeedPlan(amr_trajectory_msgs::amr_trajectory_msg &trajectory, float acc, float dcc)
{
    for (auto &m : um_record_)
    {
        float a = 0.5f;
        float d = -0.5f;
        float vmax = m.second.max_speed;
        float vs = 0.0f;
        float ve = 0.0f;
        int start = m.first;

        trap_.TrapezoidalCalcSpeed(trajectory.points, start, m.second.end, a, d, vs, ve, vmax, (m.second.move_direction=="forward"?1:-1));
    }
    um_record_.clear();
}
